/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#include "OwnGWSMeasure3D.hpp"

#include <rw/math/EAA.hpp>
#include <rw/math/Vector3D.hpp>
#include <rw/math/Constants.hpp>
#include <boost/foreach.hpp>
#include <iostream>

#include <rw/geometry/GiftWrapHull3D.hpp>
#include <rw/geometry/STLFile.hpp>

#include <fstream>

using namespace rw::math;
using namespace rw::geometry;
using namespace rw::graspplanning;


namespace {

/**
 * calculate the normalized friction cone from a force and friction
 * @param force
 * @param mu
 * @return
 */
std::vector<Vector3D<> > getCone(const Vector3D<>& normal, double nforce, double mu, int resolution){
	Vector3D<> unormal = normalize(normal);
	//double angle = atan(mu);
	//tan(angle)*1
	// create a vector that is tangent to the normal
	const double EPSILON = 0.000001;
	Vector3D<> tdir;
	if( fabs(normal(0))<EPSILON && fabs(normal(1))<EPSILON ){
		tdir = normalize(  Vector3D<>(0,-normal(2),normal(1)) );
	} else {
		tdir = normalize(  Vector3D<>(-normal(1),normal(0),0) );
	}
	Vector3D<> tdirScaled = tdir * (nforce * mu);

	Vector3D<> fnormal = unormal * nforce;
	std::vector<Vector3D<> > coneVerts;
	// rotate the vector around the normal in some fixed steps
	Vector3D<> P = tdirScaled; // vector to rotate
	Vector3D<> A = unormal; // vector to rotate about

	EAA<> eaa(unormal, 2*Pi/resolution );
	Rotation3D<> rot( eaa.toRotation3D() );
	for(int i=0; i<resolution-1; i++){
		coneVerts.push_back(fnormal+P);
		P = rot*P;
	}
	return coneVerts;
}

}

OwnGWSMeasure3D::OwnGWSMeasure3D(int resolution, bool useUnitVectors):
_chullCalculator( rw::common::ownedPtr(new OwnQHullND<6>() ) ),
_resolution(resolution),
_useUnitVectors(useUnitVectors),
_lambda(1.0/0.1)
{
}


double OwnGWSMeasure3D::quality(const rw::graspplanning::Grasp3D& grasp) const {
	//static int id = 0;
	std::vector< VectorND<6> > vertices;
	// first we add
	//vertices.push_back( VectorND<6>::zero() );
	BOOST_FOREACH(const rw::sensor::Contact3D& c, grasp.contacts ){
		// std::cout  << "get cone: " << c.n << " " << c.normalForce << std::endl;
		double normalForce = 1.0;
		if(!_useUnitVectors)
			normalForce = c.normalForce;

		if(normalForce<0.0001){
			RW_WARN("Normal force too small! : " << normalForce);
			continue;
		}

		Vector3D<> arm = c.p - _objCenter;
		std::vector<Vector3D<> > verts = getCone(c.n,normalForce,c.mu,_resolution);
		BOOST_FOREACH(const Vector3D<> &force, verts){
			Vector3D<> torque =  cross(arm,force);
			VectorND<6> vertice;
			vertice[0] = force[0];
			vertice[1] = force[1];
			vertice[2] = force[2];
			vertice[3] = _lambda * torque[0];
			vertice[4] = _lambda * torque[1];
			vertice[5] = _lambda * torque[2];
			vertices.push_back(vertice);
		}
	}
	//BOOST_FOREACH(VectorND<6>& v, vertices){
	//    std::cout << "--- v: " << v << "\n";
	//}


	// first do the force space
	_chullCalculator->rebuild( vertices );
	VectorND<6> or6D = VectorND<6>::zero();
	_minWrench = _chullCalculator->getMinDistInside( or6D, vertices);
	if (_minWrench > 0) {
		VectorND<6> subspaceOrigo = _chullCalculator->getMinWrench(or6D);
		_chullCalculator->subspace( vertices, subspaceOrigo );
		//std::cout << "a" << std::endl;
		//_chullCalculator->rebuild(_chullCalculator->getHullVertices());
		//std::cout << "b" << std::endl;
		//vertices = _chullCalculator->getHullVertices();
		//std::cout << "c" << std::endl;

		std::vector<VectorND<6> > vertList = _chullCalculator->getHullVertices();
		std::vector<Vector3D<> > vertList2;
		std::vector<VectorND<3> > vertListN2;
		for (size_t i = 0; i < vertList.size(); i++) {
			Vector3D<> vert;
			VectorND<3> vertN;
			vert[0] = vertList[i][0];
			vert[1] = vertList[i][1];
			vert[2] = vertList[i][2];
			vertN[0] = vertList[i][0];
			vertN[1] = vertList[i][1];
			vertN[2] = vertList[i][2];
			vertList2.push_back(vert);
			vertListN2.push_back(vertN);
			//std::cout << vert << std::endl;
		}

		/*Vector3D<> vertex1, vertex2, vertex3, vertex4, vertex5, vertex6, vertex7, vertex8;
		vertex1[0] = 4.;
		vertex1[1] = -1.;
		vertex1[2] = 1.;
		vertex2[0] = 4.;
		vertex2[1] = -1.;
		vertex2[2] = -1.;
		vertex3[0] = 6.;
		vertex3[1] = 1.;
		vertex3[2] = 1.;
		vertex4[0] = 6.;
		vertex4[1] = 1.;
		vertex4[2] = -1.;
		vertex5[0] = -1.;
		vertex5[1] = 4.;
		vertex5[2] = 1.;
		vertex6[0] = -1.;
		vertex6[1] = 4.;
		vertex6[2] = -1.;
		vertex7[0] = 1.;
		vertex7[1] = 6.;
		vertex7[2] = 1.;
		vertex8[0] = 1.;
		vertex8[1] = 6.;
		vertex8[2] = -1.;
		vertList2.push_back(vertex1);
		vertList2.push_back(vertex2);
		vertList2.push_back(vertex3);
		vertList2.push_back(vertex4);
		vertList2.push_back(vertex5);
		vertList2.push_back(vertex6);
		vertList2.push_back(vertex7);
		vertList2.push_back(vertex8);*/
		/*std::cout << vertList2.size();
		std::vector<Vector3D<> >::iterator it = vertList2.begin();
		double tol = 0.005;
		int curPos = 0;
		while(it != vertList2.end()) {
			Vector3D<> cur = *it;
			bool erased = false;
			for (size_t j=curPos+1; j < vertList2.size() && !erased; j++) {
				double val1 = cur[0]-vertList2[j][0];
				if (val1 < 0) val1 = -val1;
				double val2 = cur[1]-vertList2[j][1];
				if (val2 < 0) val2 = -val2;
				double val3 = cur[2]-vertList2[j][2];
				if (val3 < 0) val3 = -val3;
				if ((val1+val2+val3)<tol) {
					vertList2.erase(it);
					erased = true;
				}
			}
			if (!erased) {
				curPos++;
				it++;
			}
		}*/

		rw::common::Ptr<rw::geometry::OwnQHullND<3> > hull2 = rw::common::ownedPtr(new rw::geometry::OwnQHullND<3>() );
		hull2->rebuild(vertListN2);

		rw::math::Vector3D<> center( boost::numeric::ublas::zero_vector<double>(3) );
		rw::math::VectorND<3> centerN( boost::numeric::ublas::zero_vector<double>(3) );
		BOOST_FOREACH(const rw::math::Vector3D<>& vertice, vertList2){
			center[0] += vertice[0];
			center[1] += vertice[1];
			center[2] += vertice[2];
			centerN[0] += vertice[0];
			centerN[1] += vertice[1];
			centerN[2] += vertice[2];
		}
		Vector3D<> origin = Vector3D<>::zero();
		VectorND<3> originN = VectorND<3>::zero();

		_minWrench = hull2->getMinDistInside(originN);
		_avgWrench = hull2->getMinDistInside(centerN);

		rw::common::Ptr<rw::geometry::GiftWrapHull3D > hull = rw::common::ownedPtr(new rw::geometry::GiftWrapHull3D() );
		hull->rebuild(vertList2);
		char str[40] = "tmp/trimesh.stl";
		//sprintf(str, "tmp/trimesh%d.stl", (int)vertList2.size());
		//sprintf(str, "tmp/trimesh%d.stl", id++);
		const std::string file = str;
		rw::geometry::TriMesh* mesh = hull->toTriMesh();
		rw::geometry::STLFile::save(*mesh,file);
	} else {
		_minWrench = 0;
		_avgWrench = 0;
	}


	/*
	if (!_useUnitVectors) {

		std::vector<VectorND<3> > vertices2;

		double maxN;
		if (!_useUnitVectors)
			maxN = 5.0;
		else
			maxN = 1.0;
		double res = 50;
		double dN = maxN/10;
		if (_minWrench > 0) {
			for (int i = -res; i <= res; i++) {
				for (int j = -res; j <= res; j++) {
					for (int n = -res; n <= res; n++) {
						VectorND<3> vec = VectorND<3>::zero();
						vec[0] = i*dN;
						vec[1] = j*dN;
						vec[2] = n*dN;
						VectorND<6> cvec = VectorND<6>::zero();
						cvec[0] = vec[0];
						cvec[1] = vec[1];
						cvec[2] = vec[2];
						if (_chullCalculator->isInside(cvec, vertices)) {
							vertices2.push_back(vec);
							i++;
						}
					}
				}
			}
		}

		if (vertices2.size() > 3) {
			OwnQHullND<3> *chull = new OwnQHullND<3>();
			chull->rebuild(vertices2);

			VectorND<3> origin2 = VectorND<3>::zero();
			rw::math::VectorND<3> center2( boost::numeric::ublas::zero_vector<double>(3) );
			BOOST_FOREACH(const rw::math::VectorND<3>& vertice, chull->getHullVertices()){
				center2 += vertice;
			}
			center2 = center2/chull->getHullVertices().size();

			_minWrench = chull->getMinDistInside( origin2, vertices2);
			_avgWrench = chull->getMinDistInside( center2, vertices2);
		} else {
			_minWrench = 0;
			_avgWrench = 0;
		}

	}
	 */


	_isInside = _minWrench>=0;

	return _minWrench;
}
